// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-

#if OSD_PROTOCOL == OSD_REMZIBI

#if OSD_PORT == 3
#define SendSer		Serial3.print
#define SendSerln	Serial3.println
#elif OSD_PORT == 2
#define SendSer		Serial2.print
#define SendSerln	Serial2.println
#elif OSD_PORT == 1
#define SendSer		Serial1.print
#define SendSerln	Serial1.println
#else
#define SendSer		Serial.print
#define SendSerln	Serial.println
#endif

byte oldOSDSwitchPosition = 1;

void read_osd_switch()
{
	byte osdSwitchPosition = readOSDSwitch();
        Serial.print ("OSD ");
        Serial.println (osdSwitchPosition,DEC);
	if (oldOSDSwitchPosition != osdSwitchPosition){
		
		switch(osdSwitchPosition)
		{
			case 1: // First position 
			set_osd_mode(1);//on
			break;

			case 2:
			set_osd_mode(0);//off
			break;
		}
		oldOSDSwitchPosition = osdSwitchPosition;
	}
}

byte readOSDSwitch(void){
  	int osdPulseWidth = APM_RC.InputCh(OSD_MODE_CHANNEL - 1);
	if (osdPulseWidth >= 1450)  return 2;	// Off
	return 1;
}

void set_osd_mode(int mode){
		switch(mode)
		{
			case 1: // On
                        SendSerln("$CLS");
                        SendSerln("$L1");
			break;

			case 0: // Off
			SendSerln("$L0");
                        SendSerln("$CLS");
			break;
		}
}

void print_osd_data(void)
{
	SendSer("$A,");
	SendSer((float)current_loc.lat/10000000,5); //Latitude
	SendSer(",");
	SendSer((float)current_loc.lng/10000000,5); //Longitude
	SendSer(",");
	SendSer(g_gps->num_sats,DEC); //Satellite Count
	SendSer(",");
/*        if (control_mode >= AUTO) {
          SendSer((int)(wp_distance)); //Distance to Waypoint otherwise remzibi calculate from home
        } */
	SendSer(",");
	SendSer(current_loc.alt/100,DEC); //Altitude
	SendSer(",");
        SendSer( ((g_gps->ground_speed/100) / 1.852) ,DEC); //Ground Speed to km/h
	SendSer(",");
        SendSer(g_gps->ground_course / 100,DEC);
         SendSer(",");
        //Azimut
	SendSer(",");
	//SendSer(GPS.date,DEC); //Date
        SendSer(""); //Date
	SendSer(",");
	SendSer(g_gps->time,DEC); //Time
        SendSerln();

        if(BATTERY_EVENT)
        {                
                if(battery_voltage < LOW_VOLTAGE)
                {
                        SendSer("$M");
                        SendSer("97");
                        SendSer("02");
                        SendSer("D7");
                        SendSer("00");
                        SendSer(battery_voltage,1);
                        SendSerln();
                        
                        SendSer("$M");
                        SendSer("87"); // Hex Column (01h - 1Eh) for small fonts add 80h (81h - 9Eh) 
                        SendSer("0B"); // Hex Row (01h - 0Dh for NTSC, 01h - 10h for PAL) 
                        SendSer("D7"); // Hex address of First Character
                        SendSer("D7"); // Hex address of Last Character
                        SendSer("LOW VOLTAGE ALERT");
                        SendSerln();                  
                }
                else
                {
                        SendSer("$M");
                        SendSer("97");
                        SendSer("02");
                        SendSer("D5");
                        SendSer("00");
                        SendSer(battery_voltage,1);
                        SendSerln();         
                }
        }
        
        SendSer("$M");
        SendSer("83"); // Hex Column (01h - 1Eh) for small fonts add 80h (81h - 9Eh) 
        SendSer("0D"); // Hex Row (01h - 0Dh for NTSC, 01h - 10h for PAL) 
        SendSer("E9"); // Hex address of First Character
        SendSer("00"); // Hex address of Last Character


	switch (control_mode){
		case MANUAL:
			SendSer("MANUAL          ");
                       break;
		case STABILIZE:
			SendSer("STABILIZE       ");
                        break;
		case CIRCLE:
			SendSer("CIRCLE          ");
                        break;
		case FLY_BY_WIRE_A:
			SendSer("FBW:A      ");
                        break;
		case FLY_BY_WIRE_B:
			SendSer("FBW:B      ");
                        break;
		case AUTO:
			SendSer("WP");
                        SendSer((int)next_wp_index);
                        SendSer(":");
                        SendSer((int)(wp_distance)*OSD_UNIT);
                        SendSer("   ");
                        break;
		case RTL:
			SendSer("RTL:");
                        SendSer((int)(wp_distance)*OSD_UNIT);
                        SendSer("   ");
                        break;
		case LOITER:
			SendSer("LOITER   ");
                        break;
		case TAKEOFF:
			SendSer("TAKEOFF   ");
                        break;
		case LAND:
			SendSer("LAND      ");
                        break;
        }
        SendSerln("");
}

void print_osd_ahrs(void)
{
	SendSer("$I,");
	SendSer(dcm.roll_sensor/100,DEC); //Roll
        SendSer(",");
	SendSer(dcm.pitch_sensor/100,DEC); //Pitch
	SendSer(",");
        SendSerln();
} 

void osd_init_home(void)
{
        SendSer("$SH");
        SendSerln();
        SendSer("$CLS");
        SendSerln(); 
}

#endif

